/**********************************************************************************************************************
 * @file    sensor_support_icm42670.c
 * @author  Queclink Billy.Luo
 * @date    2021-6-1
 * @brief   icm42670抽象实现接口
 *
 * Copyright (C) 2021 Queclink Wireless Solutions (ShenZhen) Co., Ltd. All Rights Reserved.
 *
 * @attention
 *********************************************************************************************************************/
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>

#include <public/errno.h>
#include <sensor/icm42670/icm42670.h>
#include <sensor/icm42670/sensor_icm42670.h>
#include <sensor/sensor_wrapper.h>
#include <sensor/sensor_support.h>
#include <sensor/sensor_log.h>
#include "task/gsensor/gsensor.h"


static int icm42670_interface_write(unsigned char addr, unsigned char *data, uint16_t cnt);
static int icm42670_interface_read(unsigned char addr, unsigned char *data, uint16_t cnt);

static context_t icm42670_ctx = {
    .chip_id    = 0,
    .write_reg  = icm42670_interface_write,
    .read_reg   = icm42670_interface_read,
    .delay_ms   = sensor_delay,
};


static int icm42670_interface_write(unsigned char addr, unsigned char *data, uint16_t cnt)
{
    return sensor_spi_write_reg(addr & 0x7F, 1, data, cnt);   
}

static int icm42670_interface_read(unsigned char addr, unsigned char *data, uint16_t cnt)
{
    return sensor_spi_read_reg(addr | 0x80, 1, data, cnt);

}


static uint8_t icm42670_accel_range_get(void)
{
    uint8_t range = ACCEL_RANGE_2G;
    switch (qlsdk_sensor_support.accel_range) {
        case 2:
            range = ACCEL_RANGE_2G;
            break;

        case 4:
            range = ACCEL_RANGE_4G;
            break;

        case 8:
            range = ACCEL_RANGE_8G;
            break;

        case 16:
            range = ACCEL_RANGE_16G;
            break;

        default:
            range = ACCEL_RANGE_2G;
            break;
    }

    return range;
}


static uint8_t icm42670_gyro_range_get(void)
{
    uint8_t range = GYRO_RANGE_250DPS;

    switch(qlsdk_sensor_support.gyro_range) {
    case 250:
        range = GYRO_RANGE_250DPS;
        break;
    case 500:
        range = GYRO_RANGE_500DPS;
        break;
    case 1000:
        range = GYRO_RANGE_1000DPS;
        break;
    case 2000:
        range = GYRO_RANGE_2000DPS;
        break;
    default:
        range = GYRO_RANGE_250DPS;
        break;
    }

    return range;
}

static uint8_t icm42670_odr_get(void)
{
    uint8_t odr;

    switch(qlsdk_sensor_support.cur_odr) {
    case 100:
        odr = ODR_100HZ;
        break;
    case 200:
        odr = ODR_200HZ;
        break;
    case 400:
        odr = ODR_400HZ;
        break;
    default:
        odr = ODR_100HZ;
        break;
    }

    return odr;
}


int icm42670_accel_range_set(int nRange)
{
    int ret = 0;
    int code = 0;
    icm_reg_t reg;
    int prev_accel_range;

    if ( (nRange != 2) && (nRange != 4) && (nRange != 8) && (nRange != 16) )
        return -1;

    prev_accel_range = qlsdk_sensor_support.accel_range;
    qlsdk_sensor_support.accel_range = nRange;

    reg.byte = 0x00;
    reg.accel_conf0.range = icm42670_accel_range_get();
    reg.accel_conf0.odr = icm42670_odr_get();
    code = icm42670_reg_set(&icm42670_ctx, ACCEL_CONFIG0_REG, reg.byte);
    if(0 != code) {
        ret = -1;
        qlsdk_sensor_support.accel_range = prev_accel_range;
    }

    return ret;

}


int icm42670_odr_set(int frequency) //frequency  100, 200, 400;
{
    int ret = 0;
    int code = 0;
    icm_reg_t reg;
    int prev_frequency;

    if(100 != frequency && 200 != frequency && 400 != frequency)
        return -1;

    prev_frequency = qlsdk_sensor_support.cur_odr;
    qlsdk_sensor_support.cur_odr = frequency;

    reg.byte = 0x00;
    reg.accel_conf0.range = icm42670_accel_range_get();
    reg.accel_conf0.odr = icm42670_odr_get();
    code = icm42670_reg_set(&icm42670_ctx, ACCEL_CONFIG0_REG, reg.byte);
    if(0 != code) {
        ret = -1;
        qlsdk_sensor_support.cur_odr = prev_frequency;
    }

    return ret;
}
 

int icm42670_accel_sensitivity_set(int sense_value)
{
    int ret = 0;
    int code[3] = {0};
    icm_reg_t reg;

    if ( (sense_value < 0) || (sense_value > 255))
        return -1;

      reg.byte = sense_value;
      code[0] = bank_reg_set(&icm42670_ctx, ICM_BANK1, WOMX_THR_REG, reg.byte);
      if(0 != code[0])
          ret = -1;
      
      code[1] = bank_reg_set(&icm42670_ctx, ICM_BANK1, WOMY_THR_REG, reg.byte);
      if(0 != code[1])
          ret = -1;
      
      code[2] = bank_reg_set(&icm42670_ctx, ICM_BANK1, WOMZ_THR_REG, reg.byte);
      if(0 != code[2])
          ret = -1;
      
      if(ret != 0)
            sensor_trace(LOG_ERR "icm42670 accel_sensitivity update fail %d,(%d,%d,%d).\r\n", ret, \
                          code[0], code[1], code[2]);
    

    return ret;

}

/**
 * @brief 清除中断标志
 */
void clear_wom_int_flags(void)
{
    icm_reg_t reg;
    
    icm42670_reg_get(&icm42670_ctx, INT_STATUS2_REG, &reg.byte);
}

static uint8_t icm42670_wm_get(void)
{
    uint8_t wm;

    switch(qlsdk_sensor_support.cur_odr) {
    case 100:
        wm = 5;
        break;
    case 200:
        wm = 10;
        break;
    case 400:
        wm = 20;
        break;
    default:
        wm = 5;
        break;
    }

    return wm;
}


static int icm42670_initialize(void)
{
    int ret = 0;
    int code[20] = { 0 };
    icm_reg_t reg;

    code[1] = icm42670_interface_read(0x75, &reg.byte, 1);
    if(0 != code[1] || (0x60 != reg.byte)) {
        ret = -1;
        icm42670_ctx.chip_id = 0;
        if (0 == code[1])
            code[1] = -200;
    } else {
        icm42670_ctx.chip_id = reg.byte;
    }

    reg.byte = 0x00;
    reg.signal_path.soft_reset = 1;
    reg.signal_path.fifo_flush = 1;
    code[2] = icm42670_reg_set(&icm42670_ctx, SIGNAL_PATH_REG, reg.byte);
    if(0 != code[2])
        ret = -1;
    icm42670_ctx.delay_ms(5);

    reg.byte = 0x00;
    reg.pwr_mgmt.idle = 1;
    reg.pwr_mgmt.acc_mode = ACC_MODE_OFF;
    reg.pwr_mgmt.gyro_mode = GYRO_MODE_OFF;
    code[3] = icm42670_reg_set(&icm42670_ctx, PWR_MGMT0_REG, reg.byte);
    if(0 != code[3])
        ret = -1;
    icm42670_ctx.delay_ms(1);

    reg.byte = 0x00;
    reg.int_conf.int1_polarity = ACTIVE_HIGH;
    reg.int_conf.int1_circuit = PUSH_PULL;
    reg.int_conf.int1_mode = LATCH_MODE;
    code[4] = icm42670_reg_set(&icm42670_ctx, INT_CONFIG_REG, reg.byte);
    if(0 != code[4])
        ret = -1;

    reg.byte = 0x00;
    reg.gyro_conf0.range = icm42670_gyro_range_get();
    reg.gyro_conf0.odr = icm42670_odr_get();
    code[5] = icm42670_reg_set(&icm42670_ctx, GYRO_CONFIG0_REG, reg.byte);
    if(0 != code[5])
        ret = -1;

    reg.byte = 0x00;
    reg.accel_conf0.range = icm42670_accel_range_get();
    reg.accel_conf0.odr = icm42670_odr_get();
    code[6] = icm42670_reg_set(&icm42670_ctx, ACCEL_CONFIG0_REG, reg.byte);
    if(0 != code[6])
        ret = -1;

    code[7] = icm42670_fifo_wm_level_set(&icm42670_ctx, icm42670_wm_get());
    if(0 != code[7])
        ret = -1;

    reg.byte = 0x00;
    reg.intf_conf0.fifo_record = FIFO_CNT_RECORD;
    reg.intf_conf0.fifo_endian = BIG_ENDIAN;
    reg.intf_conf0.data_endian = BIG_ENDIAN;
    code[8] = icm42670_reg_set(&icm42670_ctx, INTF_CONFIG0_REG, reg.byte);
    if(0 != code[8])
        ret = -1;

    reg.byte = 0x00;
    reg.tmst_conf1.delta_en = 0;
    reg.tmst_conf1.fsync_en = 0;
    reg.tmst_conf1.tmst_en = ICM_ENABLE;
    code[9] = bank_reg_set(&icm42670_ctx, ICM_BANK1, TMST_CONFIG1_REG, reg.byte);
    if(0 != code[9])
        ret = -1;

    reg.byte = 0x00;
    reg.fifo_conf5.fifo_20bit_en = ICM_DISABLE;
    reg.fifo_conf5.partial_rd = ICM_DISABLE;
    reg.fifo_conf5.wm_gt_th = 1;
    reg.fifo_conf5.tmst_fsync_en = ICM_DISABLE;
    reg.fifo_conf5.fifo_acc_en = ICM_ENABLE;
    reg.fifo_conf5.fifo_gyro_en = ICM_ENABLE;
    code[10] = bank_reg_set(&icm42670_ctx, ICM_BANK1, FIFO_CONFIG5_REG, reg.byte);
    if(0 != code[10])
        ret = -1;

    reg.byte = (int)(sensor_motion_accel_threshold_get() * (ICM42670_MAX_ACCEL_VALUE*1.0f / get_gsensor_local_run_config()->measure_range));
    code[11] = bank_reg_set(&icm42670_ctx, ICM_BANK1, WOMX_THR_REG, reg.byte);
    if(0 != code[11])
        ret = -1;

    code[12] = bank_reg_set(&icm42670_ctx, ICM_BANK1, WOMY_THR_REG, reg.byte);
    if(0 != code[12])
        ret = -1;

    code[13] = bank_reg_set(&icm42670_ctx, ICM_BANK1, WOMZ_THR_REG, reg.byte);
    if(0 != code[13])
        ret = -1;

    reg.byte = 0x00;
    reg.fifo_conf1.fifo_bypass = ICM_DISABLE;
    reg.fifo_conf1.fifo_mode = STREAM_MODE;
    code[14] = icm42670_reg_set(&icm42670_ctx, FIFO_CONFIG1_REG, reg.byte);
    if(0 != code[14])
        ret = -1;

    reg.byte = 0x00;
    reg.int_src0.fifo_wm_int1 = ICM_DISABLE; 
    reg.int_src0.fifo_full_int1 = ICM_DISABLE; 
    code[15] = icm42670_reg_set(&icm42670_ctx, INT_SRC0_REG, reg.byte);
    if(0 != code[15])
        ret = -1;

    reg.byte = 0x00;
    reg.pwr_mgmt.acc_mode = ACC_MODE_LN;
    reg.pwr_mgmt.gyro_mode = GYRO_MODE_LN;
    reg.pwr_mgmt.idle = 0;
    reg.pwr_mgmt.lp_clk_sel = 0;
    code[16] = icm42670_reg_set(&icm42670_ctx, PWR_MGMT0_REG, reg.byte);
    if(0 != code[16])
        ret = -1;

    if (0 != ret) {
        sensor_trace(LOG_ERR "icm42670 init fail %d,(%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d).\r\n", ret,
                           code[0], code[1], code[2], code[3], code[4], code[5], code[6], code[7], code[8], code[9], code[10], code[11], code[12], code[13], code[14], code[15], code[16]);
    }

    icm42670_ctx.delay_ms(50);
    return ret;
}

int icm42670_sensor_initialize(void)
{
    int i = 0;

    /* 避免出现初始化失败问题，多次重试 */
    for (i = 0; i < INIT_MAX_COUNT; i++) {
        if (0 == icm42670_initialize())
            break;
    }
    if (i < INIT_MAX_COUNT)
        return 0;
    else
        return -1;
}


static int icm42670_lowpower_initialize(void)
{
    int ret = 0;
    int code[13] = { 0 };
    icm_reg_t reg;

    /* 先配置为LOW NOISE模式，否则MREG1寄存器在LP模式下会出问题 */
    reg.byte = 0x00;
    reg.pwr_mgmt.acc_mode = ACC_MODE_LN;
    reg.pwr_mgmt.gyro_mode = GYRO_MODE_LN;
    reg.pwr_mgmt.idle = 0;
    reg.pwr_mgmt.lp_clk_sel = 1;
    code[0] = icm42670_reg_set(&icm42670_ctx, PWR_MGMT0_REG, reg.byte);
    if(0 != code[0])
        ret = -1;
    icm42670_ctx.delay_ms(1);

    reg.byte = 0x00;
    reg.fifo_conf1.fifo_bypass = ICM_ENABLE;
    reg.fifo_conf1.fifo_mode = STREAM_MODE;
    code[1] = icm42670_reg_set(&icm42670_ctx, FIFO_CONFIG1_REG, reg.byte);
    if(0 != code[1])
        ret = -1;
    icm42670_ctx.delay_ms(1);

    /* 06H配置INT1中断类型 */
    reg.byte = 0x00;
    reg.int_conf.int1_polarity = 1;  //active High
    reg.int_conf.int1_circuit = 1;  //push pull
    reg.int_conf.int1_mode = 1;     //latched mode
    code[2] = icm42670_reg_set(&icm42670_ctx, INT_CONFIG_REG, reg.byte);
    if(0 != code[2])
        ret = -1;
    icm42670_ctx.delay_ms(1);
    
    /* 2BH关闭INT1其他中断映射 */
    reg.byte = 0x00;
    reg.int_src0.fifo_wm_int1 = ICM_DISABLE;
    reg.int_src0.fifo_full_int1 = ICM_DISABLE;
    code[3] = icm42670_reg_set(&icm42670_ctx, INT_SRC0_REG, reg.byte);
    if(0 != code[3])
        ret = -1;
    icm42670_ctx.delay_ms(1);

    /* LP下ACC按照最低频率和量程配置 */
    reg.byte = 0x00;
    reg.accel_conf0.range = ACCEL_RANGE_2G;
    reg.accel_conf0.odr = ODR_100HZ;
    code[4] = icm42670_reg_set(&icm42670_ctx, ACCEL_CONFIG0_REG, reg.byte);
    if(0 != code[4])
        ret = -1;
    icm42670_ctx.delay_ms(1);

    reg.byte = 0x00;
    reg.apex_conf0.dmp_init = 1;
    reg.apex_conf0.mem_reset = 1;
    reg.apex_conf0.power_save = 1;
    code[5] = icm42670_reg_set(&icm42670_ctx, APEX_CONFIG0_REG, reg.byte);
    if(0 != code[5])
        ret = -1;
    icm42670_ctx.delay_ms(1);

    reg.byte = 0x00;
    reg.apex_conf1.dmp_odr = DMP_ODR_100HZ;
    reg.apex_conf1.ff_en = ICM_DISABLE;
    reg.apex_conf1.ped_en = ICM_DISABLE;
    reg.apex_conf1.smd_en = ICM_DISABLE;
    reg.apex_conf1.tilt_en = ICM_DISABLE;
    code[6] = icm42670_reg_set(&icm42670_ctx, APEX_CONFIG1_REG, reg.byte);
    if(0 != code[6])
        ret = -1;
    icm42670_ctx.delay_ms(1);
    
    reg.byte = (int)(sensor_wakeup_accel_threshold_get() * (ICM42670_MAX_ACCEL_VALUE *1.0f / get_gsensor_local_run_config()->measure_range));  //default 0.2G
    code[7] = bank_reg_set(&icm42670_ctx, ICM_BANK1, WOMX_THR_REG, reg.byte);
    if(0 != code[7])
        ret = -1;
  
    code[8] = bank_reg_set(&icm42670_ctx, ICM_BANK1, WOMY_THR_REG, reg.byte);
    if(0 != code[8])
        ret = -1;
  
    code[9] = bank_reg_set(&icm42670_ctx, ICM_BANK1, WOMZ_THR_REG, reg.byte);
    if(0 != code[9])
        ret = -1;
  
    icm42670_ctx.delay_ms(1);

    /* wom中断映射到INT1(XYZ轴) */
    reg.byte = 0x00;
    reg.int_src1.wom_x_int1 = ICM_ENABLE;
    reg.int_src1.wom_y_int1 = ICM_ENABLE;
    reg.int_src1.wom_z_int1 = ICM_ENABLE;
    code[10] = icm42670_reg_set(&icm42670_ctx, INT_SRC1_REG, reg.byte);
    if(0 != code[10])
        ret = -1;
    icm42670_ctx.delay_ms(1);

    /* 进入ACC的LP触发模式 */
    reg.byte = 0x00;
    reg.pwr_mgmt.acc_mode = ACC_MODE_LP;
    reg.pwr_mgmt.gyro_mode = GYRO_MODE_OFF;
    //reg.pwr_mgmt.gyro_mode = GYRO_MODE_STANDBY;
    reg.pwr_mgmt.idle = 0;
    reg.pwr_mgmt.lp_clk_sel = 0;
    code[11] = icm42670_reg_set(&icm42670_ctx, PWR_MGMT0_REG, reg.byte);
    if(0 != code[11])
        ret = -1;
    icm42670_ctx.delay_ms(1);

    /* 0x27H开启WOM功能 */
    reg.byte = 0x00;
    reg.wom_conf.wom_en = ICM_ENABLE;
    reg.wom_conf.wom_mode = CMP_TO_PREV;
    reg.wom_conf.int_mode = LOGIC_OR;
    reg.wom_conf.int_dur = 1;
    code[12] = icm42670_reg_set(&icm42670_ctx, WOM_CONFIG_REG, reg.byte);
    if(0 != code[12])
        ret = -1;
    icm42670_ctx.delay_ms(1);

    if (0 != ret) {
        sensor_trace(LOG_ERR "icm42670 init(low power) fail %d,(%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d).\r\n", ret,
                           code[0], code[1], code[2], code[3], code[4], code[5], code[6], code[7], code[8], \
                           code[9], code[10], code[11], code[12]);
    }

    return ret;
}

int icm42670_testreg_read(void)
{
    int ret = 0;
    int code[10] = { 0 };
    uint8_t reg_value[10] = {0};
    icm_reg_t reg;

    uint8_t check_reg_addr[] = {0x2B, 0x2C, 0x39, 0x3A, 0x3B, 0x3C};
    
    for(int i=0; i<sizeof(check_reg_addr); i++) {
        reg.byte = 0x00;
        code[i] = icm42670_reg_get(&icm42670_ctx, check_reg_addr[i], &reg.byte);
        if(0 != code[i])
            ret = -1;
        reg_value[i] = reg.byte;
        //icm42670_ctx.delay_ms(1);
    }

    if (0 != ret) {
        sensor_trace(LOG_ERR "icm42670 read fail %d,(%d,%d,%d,%d,%d,%d).\r\n", ret,
                           code[0], code[1], code[2], code[3], code[4], code[5]);
    }else {
        sensor_trace(LOG_INFO "icm42670 read success %d,(2B:0x%02X,2C:0x%02X,39:0x%02X,3A:0x%02X,3B:0x%02X,3C:0x%02X).\r\n", ret,
                           reg_value[0], reg_value[1], reg_value[2], reg_value[3], reg_value[4], reg_value[5]);
    }

    return ret;
}


int icm42670_sensor_lowpower_initialize(void)
{
    int i = 0;

    /* 避免出现初始化失败问题，多次重试 */
    for (i = 0; i < INIT_MAX_COUNT; i++) {
        if (0 == icm42670_lowpower_initialize())
            break;
    }
    if (i < INIT_MAX_COUNT)
        return 0;
    else
        return -1;
}

int icm42670_sensor_chip_id_get(void *id)
{
    *(unsigned char *)id = icm42670_ctx.chip_id;
    return 0;
}

float icm42670_sensor_temperature_get(void)
{
    return icm42670_temperature_get(&icm42670_ctx);
}

int icm42670_sensor_accel_frame_get(void *accel)
{
    int ret = 0;
    unsigned char buf[6];
    int acc_1g = sensor_accel_1g_range_count();

    if (NULL == accel)
        return -EINVAL;

    ret = icm42670_accel_raw_get(&icm42670_ctx, buf);
    if(0 == ret) {
        ((struct acceleration *)accel)->y = ((signed short)(buf[0] << 8 | buf[1])) * -1;
        ((struct acceleration *)accel)->x = (signed short)(buf[2] << 8 | buf[3]);
        ((struct acceleration *)accel)->z = (signed short)(buf[4] << 8 | buf[5]);
        ((struct acceleration *)accel)->y -= qlsdk_sensor_support.zero_offset.x * acc_1g * -1;
        ((struct acceleration *)accel)->x -= qlsdk_sensor_support.zero_offset.y * acc_1g;
        ((struct acceleration *)accel)->z -= qlsdk_sensor_support.zero_offset.z * acc_1g;
    }

    return ret;
}

int icm42670_sensor_gyro_frame_get(void *gyro)
{
    int ret;
    uint8_t buf[6];

    if (NULL == gyro)
        return -EINVAL;

    ret = icm42670_gyro_raw_get(&icm42670_ctx, buf);
    if(0 == ret) {
        ((struct angularrate *)gyro)->y = ((signed short)(buf[0] << 8 | buf[1])) * -1;
        ((struct angularrate *)gyro)->x = (signed short)(buf[2] << 8 | buf[3]);
        ((struct angularrate *)gyro)->z = (signed short)(buf[4] << 8 | buf[5]);
    }

    return ret;
}

unsigned int icm42670_sensor_max_accel_value(void)
{
    return ICM42670_MAX_ACCEL_VALUE;
}

